/*
 * utility_gyro.c
 *
 *  Created on: 11/mar/2014
 *      Author: massimo
 */

#include <msp430x552x.h>
#include <stdio.h>
#include "gyro_init.h"
#include "init.h"

///
/// converte le letture raw in gradi
void convertInteg2Deg(gyro *gPtr, raw_data *rPtr){
	/// i dati nel girsoscopio sono su 16 bit in complemento a due
	/// rotazioni positive sono comprese tra 0 e 32767
	/// rotazioni negative tra -32768 e -1
	float tmp;
	int i;
	// Il valore impostato sono 500 gradi/s max (F.S.)
	for ( i = 0; i < 3; i++){
		tmp = rPtr->axis[i] * 500 / 32768;
		/// integra il valore su un intervallo di tempo di 50ms
		/// che corrisponde al periodo del timer
		tmp = tmp * 0.05;
		gPtr->axis[i] += tmp;
		gPtr->Aaxis[i] = (int)gPtr->axis[i];

	}

}


///
/// legge i dati grezzi dal modulo
unsigned char readFromModule(raw_data *rPtr){
	unsigned char buff[8], valore, stato;

	valore = readI2CByteFromAddress(STATUS_REG, &stato);
	if (valore != 0){
	/// legge i dati da tutti i registri del giroscopio
		stato = readI2C_N_Byte(OUT_X_L, 6, buff);
		rPtr->axis[0] = (int)((buff[1]<< 8) + buff[0]);
		rPtr->axis[1] = (int)((buff[3]<< 8) + buff[2]);
		rPtr->axis[2] = (int)((buff[5]<< 8) + buff[4]);
		return OK;
	}
	return ERROR;
}

///
/// inizializza la struttra dati del giroscopio
void initGdata(gyro *Gptr){
	Gptr->IsPresent = NOT_PRESENT;
	Gptr->IsOn =OFF;
	Gptr->axis[0] = Gptr->axis[1] = Gptr->axis[2] = 0;
}

///
/// legge il sensore (giroscopio)
void readSensor(gyro *gPtr, raw_data *rPtr){
	unsigned char valore;
	int x,y,z;
	initI2C_B1(FDCO, 800000, GYRO_ADDR);
	valore = readFromModule(rPtr);
	if (valore != ERROR){
		/// converte la lettura in gradi e la integra ad intervalli di 50 ms
		convertInteg2Deg(gPtr, rPtr);
	}
	//valore = readI2CByteFromAddress(STATUS_REG, &stato);
	x = gPtr->Aaxis[0];
	z = gPtr->Aaxis[1];
	y = gPtr->Aaxis[2];

	printf("asse x: %d\t",x);
	printf("\tasse y: %d\t",y);
	printf("\tasse z: %d\r\n",z);
	//printf("contatore: %d\r\n", contatore);


	P1OUT ^= 1;
}
